/*
 * Copyright (C) 2012 Virglinio & Asociados
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

package com.virglinio.movimientoCuerpo.opengl;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;

import com.virglinio.movimientoCuerpo.Body;
import com.virglinio.movimientocuerpo.bluetooh.ByteQueue;

import android.opengl.Matrix;
import android.os.Handler;
import android.os.Message;
import android.util.Log;

/**
 * Almacena la información como un conjunto de ángulos
 * marítimos (roll, pitch,yaw).
 *
 * Positive pitch : nose up
 * Positive roll : right wing down
 * Positive yaw : clockwise
 *
 * mas información en: http://en.wikipedia.org/wiki/Euler_angles
 *
 **/

public class EulerAngle {
	// Para Depuración
	private static final boolean DEBUG = true;
	private static final String TAG = "EulerAngle";
	
	/**
     * Our private message id, which we use to receive new input from the
     * remote process.
     */
    public static final int UPDATE = 1;

	private static final int SENSOR_1 = 1;
	private static final int SENSOR_2 = 2;
	
	private float m_roll;
	private float m_pitch;
	private float m_yaw;
	private FloatBuffer m_matrix;
	private boolean m_changed = true;
	private float [] m_buffer = new float[16];
	
	private ByteQueue byteQueue;
	
	/**
     * Used to temporarily hold data received from the remote process. Allocated
     * once and used permanently to minimize heap thrashing.
     */
	private byte[] mReceiveBuffer;

	public EulerAngle(float roll, float pitch, float yaw) {
		m_roll  = roll;
		m_pitch = pitch;
		m_yaw   = yaw;
		
		mReceiveBuffer = new byte[4 * 1024];
		byteQueue = new ByteQueue(4 * 1024);		
	}

	public synchronized EulerAngle set(float roll, float pitch, float yaw) {
		m_roll  = roll;
		m_pitch = pitch;
		m_yaw   = yaw;

		m_changed = true;

		return this;
	}
	
	@Override
	public String toString() {
		return "roll=" + m_roll + " pitch=" + m_pitch + " yaw=" + m_yaw;
	}

	public float getYaw() {
		return m_yaw;
	}

	public float getPitch() {
		return m_pitch;
	}

	public float getRoll() {
		return m_roll;
	}

	/**
	 * This code parses an Euler angle string from the 
	 * Sparkfun 9DOF Razor IMU AHRS 1.0 firmware.
	 *
	 * Expected format (only text within quotes): "!ANG:0.15,0.42,17.98\r\n"
	 *
	 * @return EulerAngle containing the angles, NULL if parsing unsuccessful
	 */
	public static EulerAngle parseLine(String line) {
		return parseLine(line, null);
	}
	
	public static EulerAngle parseLine(byte[] buffer, int sensor){ //32768
		
		int roll = UByte(buffer[0]);		
		roll = roll << 8;
		roll = roll | UByte(buffer[1]);					
		if ((roll & 32768) == 32768) {			
			roll = compA2(roll);			
		}
		// Angulo normal
				
		int pitch = UByte(buffer[2]);
		pitch = pitch << 8;
		pitch = pitch | UByte(buffer[3]);				
		if ((pitch & 32768) == 32768) {
			pitch = compA2(pitch);
		}
		
		int yaw = UByte(buffer[4]);
		yaw = yaw << 8;
		yaw = yaw | UByte(buffer[5]);				
		if ((yaw & 32768) == 32768) {
			yaw = compA2(yaw);
		}
		
		if (DEBUG) {
			Log.e(TAG, "Sensor: " + sensor + " roll: " + roll + " pitch: " + pitch + " yaw: " + yaw + " Paquete: " + buffer[6]);
		}
		
		switch (sensor) {
		case SENSOR_1:
			Body.eulerAngles[0].set(roll, pitch, yaw);
			break;
			
		case SENSOR_2:
			Body.eulerAngles[1].set(roll, pitch, yaw);
			break;

		default:
			break;
		}
			
			
		
		
		
		
		//float roll  = Float.parseFloat(strAngles[0]);
		//float pitch = Float.parseFloat(strAngles[1]);
		//float yaw   = Float.parseFloat(strAngles[2]);

		return null;
		//return (eu == null)? new EulerAngle(roll,pitch,yaw) : eu.set(roll, pitch, yaw);
		
	}
	
	private static int compA2(int angulo) {
		// TODO Auto-generated method stub
		int anguloTemp;
		anguloTemp = angulo; 
		anguloTemp = anguloTemp & 32767;
		anguloTemp -= 32768;
		return anguloTemp;
	}

	private static int UByte(byte b){
    	if(b < 0) // if negative
    		return (int)( (b & 0x7F) + 128 );
    	else
    		return (int) b;
    }

	public static EulerAngle parseLine(String line, EulerAngle eu) {
		if (DEBUG) {
				Log.e(TAG, "line " + line);
			}
		
		if (line.length() < 6 || line.substring(0, 5).compareTo("!ANG:") != 0) {
			
			return null;
		}

		String [] strAngles = line.substring(5).split(",");

		if (strAngles.length != 3) {
			if (DEBUG) {
				Log.e(TAG, "strAngles.length != 3");
			}
			return null;
		}

		if (DEBUG) {
			Log.e(TAG, "CORRECTO");
		}
		
		float roll  = Float.parseFloat(strAngles[0]);
		float pitch = Float.parseFloat(strAngles[1]);
		float yaw   = Float.parseFloat(strAngles[2]);

		return (eu == null)? new EulerAngle(roll,pitch,yaw) : eu.set(roll, pitch, yaw);
	}

	public synchronized FloatBuffer getRotMatrix() {
		if(m_matrix == null) {
			ByteBuffer storage = ByteBuffer.allocateDirect(16 * Float.SIZE / 8);
			storage.order(ByteOrder.nativeOrder());
			m_matrix = storage.asFloatBuffer();
		}

		if(!m_changed) {
			m_matrix.position(0);
			return m_matrix;
		}

		Matrix.setRotateEulerM(m_buffer, 0, m_pitch, m_yaw, m_roll);
		m_matrix.position(0);
		m_matrix.put(m_buffer);


		/* 
                // compute rotation
                float A = (float) Math.cos(Math.toRadians(m_pitch));
                float B = (float) Math.sin(Math.toRadians(m_pitch));
                float C = (float) Math.cos(Math.toRadians(m_yaw));
                float D = (float) Math.sin(Math.toRadians(m_yaw));
                float E = (float) Math.cos(Math.toRadians(m_roll));
                float F = (float) Math.sin(Math.toRadians(m_roll));
                float AD = A*D;
                float BD = B*D;

                m_matrix.position(0);

                m_matrix.put(C*E);
                m_matrix.put(-C*F);
                m_matrix.put(D);
                m_matrix.put(0);

                m_matrix.put( BD*E+A*F);
                m_matrix.put(-BD*F+A*E);
                m_matrix.put(-B*C);
                m_matrix.put(0);

                m_matrix.put(-AD*E+B*F);
                m_matrix.put( AD*F+B*E);
                m_matrix.put(A*C);
                m_matrix.put(0);

                m_matrix.put(0);
                m_matrix.put(0);
                m_matrix.put(0);
                m_matrix.put(1);
		 */


		// scroll back to the beginning
		m_matrix.position(0);

		m_changed = false;

		return m_matrix;
	}
	
	public void write(byte[] buffer, int length){
		try {
			byteQueue.write(buffer, 0, length);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		handler.sendMessage(handler.obtainMessage(UPDATE));
	}
	
	private final Handler handler = new Handler(){

		@Override
		public void handleMessage(Message msg) {
			// TODO Auto-generated method stub
			if (msg.what == UPDATE) {
				update();
			}
		}			
	};	

	private void update() {		
		int bytesAvailable = byteQueue.getBytesAvailable();		
		int bytesToRead = Math.min(bytesAvailable, mReceiveBuffer.length);
		
		if (DEBUG) {
			Log.e(TAG, "bytesToRead: " + bytesToRead);
		}		
		
		if (bytesToRead > 11) {
			try {
				int bytesRead = byteQueue.read(mReceiveBuffer, 0, 3); 

				if (mReceiveBuffer[0] == 10) {
					if (mReceiveBuffer[1] == 20) {
						if (mReceiveBuffer[2] == 1) {
							if (DEBUG) {
								Log.e(TAG, "mReceiveBuffer[0] = " + mReceiveBuffer[0]);
								Log.e(TAG, "mReceiveBuffer[1] = " + mReceiveBuffer[1]);
								Log.e(TAG, "mReceiveBuffer[2] = " + mReceiveBuffer[2]);
							}
							
							bytesRead = byteQueue.read(mReceiveBuffer, 0, 7);						
							
							if (DEBUG) {
								for (int i = 0; i < bytesRead; i++) {
									Log.e(TAG, "mReceiveBuffer["+ i + "] = " + mReceiveBuffer[i]);
								}
							}
							parseLine(mReceiveBuffer, SENSOR_1);							
						}
						if (mReceiveBuffer[2] == 2) {
							if (DEBUG) {
								Log.e(TAG, "mReceiveBuffer[0] = " + mReceiveBuffer[0]);
								Log.e(TAG, "mReceiveBuffer[1] = " + mReceiveBuffer[1]);
								Log.e(TAG, "mReceiveBuffer[2] = " + mReceiveBuffer[2]);
							}							
							
							bytesRead = byteQueue.read(mReceiveBuffer, 0, 7);							
							
							if (DEBUG) {
								for (int i = 0; i < bytesRead; i++) {
									Log.e(TAG, "mReceiveBuffer["+ i + "] = " + mReceiveBuffer[i]);
								}
							}
							parseLine(mReceiveBuffer, SENSOR_2);	
						}
					}
				}


			} catch (InterruptedException e1) {
				
				e1.printStackTrace();
			}
		}
	}
}